#include "StimDotGround.h"

#include <Random.h>

#define _USE_MATH_DEFINES // ensures macro's such as M_PI are defined
#include <math.h>

CStimDotGround::CStimDotGround(const bool& bAlsoCeiling_, const enum Stims::ObjType& eObjType_/* = Stims::GLpoint*/)
    :
CStimDot(bAlsoCeiling_? Stims::GroundCeiling : Stims::Ground, eObjType_), // set dot volume type and dot type
_bEquatedCloud(false)   // by default just a normal ground
{
}

CStimDotGround::~CStimDotGround(void)
{
}


std::string CStimDotGround::GetName()
{
    if (_eStimType==Stims::GroundCeiling)
        return std::string("Ground & Ceiling, same N at each depth");
    else
        return std::string("Ground, same N at each depth");
}

double CStimDotGround::ComputeSlice(const double& dDepth_,const int& nIdx_)
{
    double h    = dDepth_*_dTanFOVy_2;      // omit *2
    double w    = dDepth_*_dTanFOVx_2;

    if (nIdx_!=-1)
    {
        _vHalfLengthZ[nIdx_]    = dDepth_;
        _vHalfLengthY[nIdx_]    = h;        // even though Y dimension of frustum doesn't matter for cloud, when testing whether dot is inside frustum it matters, so set it
        _vHalfLengthX[nIdx_]    = w;
    }

    return w * dDepth_;                     // no need to /2 as omitted *2 above
}

void CStimDotGround::SamplePoint(const double& dRadiusZ_, CMatrix<double>& mPoint_)
{
    //// Generate point, we sample points directly into the frustum to double (in the case of ground) or triple (in the case of cloud) the chance of generating a valid point.
    //// The increase in chance offsets the extra computational load for each point (as checking whether a point is valid is not cheap either).
    //// Even when simply sampling inside the frustum, this more complicated method turns out to be faster
    //// We use inverse transform sampling to get a Z sample corresponding to a uniform density frustum. For documentation, see CreateUniformDotsIn3DFrustum in the PsychToolBox for MATLAB
    // Z
    mPoint_(2) = -sqrt(grng::_C01()*(pow(dRadiusZ_,2)-pow(_dNearPlane,2))+pow(_dNearPlane,2));      // transform to linear distribution (negate as depth position is a negative number)

    // Y
    switch (_eStimType)
    {
    case Stims::Ground:
        mPoint_(1) = -_dEyeHeight;
        break;
    case Stims::GroundCeiling:
        mPoint_(1) = grng::_C01()<.5 ? -_dEyeHeight : _dEyeHeight;
        break;
    }

    // X
    mPoint_(0) = RandLim(mPoint_(2)*_dTanFOVx_2,-mPoint_(2)*_dTanFOVx_2);

    // If _bEquatedCloud is true, the user requested the dots positions to be randomized
    // in such a way that the on-screen velocity distribution remains the same.
    // We therefore manipulate the dot positions after they have been sampled on the ground.
    // Basically, if the depth of a point (orthogonal distance to the imaging plane) is not changed,
    // we can rotate a dot around the current heading vector, thus keeping it equidistant to the heading vector,
    // and its on-screen speed will be the same.
    if (_bEquatedCloud)
    {
        double dDistFromAxis = sqrt(mPoint_(0)*mPoint_(0) + mPoint_(1)*mPoint_(1));     // compute the distance from the viewing direction axis

        // determine bounds of angles along which the dot can be placed
        //   (see IntersectCircleAndFrustumBoundingBox.m for a visualization of the algorithm):
        // the distance from the axis determines how free we are to rotate the dot.
        // if the dot is very close to the axis, we can rotate it for any amount
        // and it would not go out of the frustum
        // if the dot is in on of the 4 corners of the frustum, it can only be rotated a little
        // we now determine the minimum and maximum angle, defining the intersection
        // of a circle around the viewing axis with radius dDistFromAxis and the frustum
        // bounding box of the point at its current depth
        double dYLim = -mPoint_(2)*_dTanFOVy_2;  // Y bound frustum
        double dMinAngle,dMaxAngle;             // min and max angles defining the intersections
        if (dDistFromAxis <= dYLim)
        {
            // no intersections, angles cover the full 90 degrees
            // WARNING this assumes aspect ratio > 1
            dMinAngle = 0.;
            dMaxAngle = M_PI_2;
        }
        else
        {
            // circle does not fully fit inside frustum bounding box, find
            // intersections with right side and with top
            // top
            dMaxAngle = M_PI_2 - acos(dYLim/dDistFromAxis);

            double dXLim = -mPoint_(2)*_dTanFOVx_2;  // X bound frustum
            if (dDistFromAxis <= dXLim)
                dMinAngle = 0.;     // no intersection with vertical bounds
            else
                dMinAngle = acos(dXLim/dDistFromAxis);
        }

        // sample a random angle within these limits and sample a quadrant,
        // we now randomized the dot position, keeping it inside the frustum,
        // and have the same on screen velocity (as long as the heading or
        // viewing directions don't change)
        double  dAngle = RandLim(dMinAngle,dMaxAngle);
        int     nQuadrant = RandIdx(4);
        mPoint_(0) = dDistFromAxis*cos(dAngle);
        mPoint_(1) = dDistFromAxis*sin(dAngle);

        if (nQuadrant==1 || nQuadrant == 2) // Quadrants go counterclockwise, from 0 to 3
            mPoint_(0) *= -1.;  // Negative X
        if (nQuadrant==2 || nQuadrant == 3)
            mPoint_(1) *= -1.;  // Negative Y
    }
}

bool CStimDotGround::InsideFrustumDerived(const CMatrix<double>& mOriginPoint_)
{
    if (!_bEquatedCloud)    // the below algorithm is only for the cloud with on-screen dot speeds equated to a ground plane
        return true;
        
    // the maximum distance a dot on a ground plane can have from the viewing axis
    // is all the way at the left or right of the frustum on the plane.
    // Inside the frustum bounding box, larger distances exist.
    // for example, if the frustum half Y is 3m and half X is 4m, the furthest
    // distance is 5m. The furthest distance for a point on the ground plane
    // (or ceiling) is however only sqrt(eyeheight^2 + HalfX^2) = 4.276m.
    // here we cull points that are inside the frustum bounding box, but too
    // far away from the viewing axis.
    // If we don't perform this check, dots in a cloud could be further away
    // from the viewing axis than dots on a ground plane and would therefore
    // move too fast.

    double dDistFromAxis = sqrt(mOriginPoint_(0)*mOriginPoint_(0) + mOriginPoint_(1)*mOriginPoint_(1));     // compute the distance from the viewing direction axis
    double dHalfX        = mOriginPoint_(2)*_dTanFOVx_2;    // frustum half X length at dot's depth

    if (dDistFromAxis < dHalfX) // if fits within half X, definitely less than maximum distance
        return true;
    else
    {
        double dMaxDist      = sqrt(dHalfX*dHalfX + _dEyeHeight*_dEyeHeight);
        if (dDistFromAxis < dMaxDist)
            return true;
        else
            return false;
    }
}